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ABSTRACT 

We  describe  a  method  to  determine  (x,y,z)  position  of  a  plat¬ 
form  located  in  a  region  where  a  reference  bathymetry  map  is 
available.  The  platform  can  be  considered  an  Autonomous 
Undersea  Vehicle  (AUV)  equipped  with  a  multi-beam  high 
frequency  sonar.  Estimates  of  the  heading,  pitch  and  roll 
are  available  through  the  onboard  inertial  navigation  systems 
(INS).  An  estimate  of  the  AUV  depth  from  the  ocean  surface, 
altitude  from  a  Doppler  Velocity  Logger  (DVL)  as  well  as  the 
sound  speed  at  the  AUV  depth  are  also  available.  The  posi¬ 
tion  (x,y,  z)  is  determined  based  on  these  estimates  as  well 
as  time  delay  estimates  from  the  beam  time  series.  The  max¬ 
imum  likelihood  estimate  (MLE)  is  derived  and  connections 
between  previous  approaches  are  made.  Theoretically  based 
performance  predictions  are  compared  against  MLE  perfor¬ 
mance  in  real  data.  The  new  estimator  is  directly  linked  with 
the  relief  (or  information)  of  the  map  and  therefore  allows 
for  a  direct  estimate  of  accuracy.  This  insight  is  critical  for 
integrated  map-matching  navigation  systems  but  has  hitherto 
been  unavailable.  The  new  estimator  of  location  can  constrain 
the  error  growth  of  a  purely  INS -based  system  and  lead  to  im¬ 
proved  navigation. 

1.  INTRODUCTION 

Inertial  Navigation  Systems  onboard  the  AUV  can  provide 
high-quality  measurements  of  relative  quantities  such  as  ve¬ 
locity  and  acceleration  but  absolute  positional  error  is  still 
limited  by  initial  estimates  of  position  and  velocity  at  the  start 
of  the  mission.  Thus  absolute  positional  error  using  INS  mea¬ 
surements  cannot  be  bounded.  It  is  well  known  that  combin¬ 
ing  these  relative  measurements  with  an  absolute  positional 
measurement  in,  say  a  Kalman  Filter  framework,  can  result  in 
an  absolute  positional  error  that  is  both  unbiased  and  bounded 
(low  variance)  [1,  2].  Absolute  positional  measurements  are 
usually  obtained  through  Global  Positioning  System  (GPS) 
fixes  or  Long  Baseline  (LBL)  methods.  However,  for  certain 
long  duration  unmanned  missions  these  are  not  an  option.  If 
a  reference  bathymetry  map  is  available,  map  matching  tech¬ 
niques  can  provide  these  absolute  fixes. 

Map-matching  historically  takes  a  set  of  (x,  y )  location 


estimates  along  with  corresponding  depth  (z)  estimates  to  de¬ 
termine  the  (x,y)  position  in  the  grid,  denoted  (xo,yo,zo). 
Although  estimates,  the  (x,  y ,  z)  measurements  are  often  as¬ 
sumed  as  exact  in  the  map-matching  process.  Contouring  of 
both  the  reference  map  and  the  measurements  is  usually  the 
first  step.  The  set  of  depths  for  which  a  contour  curve  must 
be  formed  must  first  be  specified.  Both  the  measured  data 
and  the  map  must  be  contoured  in  the  same  manner.  Pixels  in 
the  measured  region  and  the  map  which  fall  on  contours  are 
considered  “on”  and  those  not  on  contours  are  “off”.  The  pro¬ 
cess  often  uses  binary  image  similarity  measures,  such  as  the 
Hausdorff  distance,  to  compare  the  suitability  of  the  match  of 
the  measured  binary  image  to  the  binary  image  map.  Efficient 
methods  to  compute  the  distances  as  well  as  search  the  space 
of  possible  locations  have  been  reported  [3,  4].  Measurement 
errors  that  were  initially  neglected  are  reconsidered  as  result¬ 
ing  in  spurious  points  in  the  binary  image.  Outlier  rejection 
methods  are  then  applied. 

The  specific  set  of  depths  chosen  for  contouring  can  dra¬ 
matically  affect  the  quality  of  the  subsequent  map-matching 
process.  As  expected,  fewer  contours  can  result  in  rapid  map¬ 
matching  but  poorer  match  quality.  The  quality  of  map-matching 
is  limited  by  the  (x,y,z)  resolution  of  the  measured  data  and 
the  map.  However  neither  source  of  error  can  be  directly  in¬ 
corporated  into  the  method.  This  is  due  to  the  fact  that  these 
methods  operate  on  binary  data.  Working  with  binary  data 
can  obscure  the  connection  between  the  local  relief  of  the  ref¬ 
erence  map  and  the  measurements.  Other  approaches  include 
estimates  of  depth  gradients  [5].  However  such  methods  also 
do  not  directly  consider  the  measurement  error.  Moreover,  for 
all  these  methods,  multiple  scans  are  required.  Relative  posi¬ 
tion  of  the  AUV  for  each  scan  must  be  known  precisely  for 
multiple  scan  methods  to  be  accurate. 

Conventional  map-matching  methods  operate  on  location 
and  depth  estimates  that  are  derived  from  the  more  fundamen¬ 
tal  quantity  -  time-delay  (or,  equivalently,  path  length  for  an 
approximately  constant  sound  speed  below  the  sonar).  As¬ 
suming  sufficiently  accurate  attitude  measurements  and  ref¬ 
erence  map,  the  fundamental  error  in  the  map-matching  pro¬ 
cess  is  the  error  in  the  path  length  measurements.  Path  length 
error  directly  determines  error  in  localization.  A  direct  ap¬ 
proach  in  terms  of  this  fundamental  quantity  is,  therefore,  of 
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interest  both  for  insight  as  well  as  for  arriving  at  more  accu¬ 
rate  estimators.  The  approach  discussed  here  can  be  viewed 
as  a  direct  maximum  likelihood  (ML)  approach.  It  is  unique 
in  that  it  provides  estimates  based  on  both  single  and  multi¬ 
ple  scans.  In  addition,  the  Cramer- Rao  Lower  Bound  (CRLB) 
can  be  determined.  The  CRLB  directly  links  achievable  map¬ 
matching  accuracy  with  local  relief  -  a  result  that  cannot  be 
determined  via  indirect  methods.  This  information  allows  for 
proper  integration  with  INS -based  estimates.  The  ML  estima¬ 
tor  is  shown  to  meet  the  CRLB  even  when  applied  to  in- water 
data. 

The  paper  is  structured  as  follows.  The  geometry  of  the 
problem  and  the  process  of  converting  INS  measurements  to 
Universal  Transverse  Mercator  (UTM)  based  measurements 
for  each  of  the  measurements  is  discussed  in  section  2.  In 
section  3,  the  reference  map,  consisting  of  gridded  (x,y,z) 
triplets,  is  interpolated  using  a  bivariate  quadratic  tensor  prod¬ 
uct  spline  [6].  The  distance  to  the  bottom  along  a  beam  can 
be  determined  via  a  nonlinear  root-finding  method.  Brent’s 
method  [7],  guaranteed  to  solve  for  the  root,  is  adopted.  The 
root  is  refined  by  recasting  the  problem  as  the  solution  of  a 
quartic  in  t ,  where  t  is  the  distance  to  the  bottom  along  the 
beam.  This  is  possible  as  the  root  lies  in  a  sub-rectangle  of 
the  spline  interpolant.  The  most  important  advantage  is  that 
the  root  can  be  determined  via  an  explicit  algebraic  equation 
which  is  amenable  to  differentiation. 

Specifically,  a  maximum  likelihood  (ML)  estimator  of  the 
true  position  (xo,yo,zo)  based  on  path  lengths  determined 
from  each  beam  time  series  is  described.  It  utilizes  the  gra¬ 
dient  of  the  log-likelihood  function  via  a  conjugate  gradient 
approach.  In  addition,  the  explicit  Cramer-Rao  Lower  Bound 
can  be  calculated  for  an  estimate  of  (xo,  yo,  zo).  These  results 
are  briefly  described  in  Section  4.  Performance  on  in- water 
data  is  quantified  in  section  5.  Section  6  discusses  conclu¬ 
sions  and  future  work. 


2.  PROBLEM  GEOMETRY 


Figure  lb  provides  an  aerial  view  of  the  bathymetric  region 
in  which  the  AUV  is  currently  residing.  The  origin  is  defined 
as  the  lower  left  corner.  The  region  need  not  be  rectangular 
but  is  drawn  that  way  for  convenience.  This  model  is  quite 
useful  as  the  Universal  Transverse  Mercator  (UTM)  coordi¬ 
nate  system  results  in  regions  that  are  approximately  rectan¬ 
gular  [8].  Figure  la  shows  a  side  view  in  which  z 5,  the  depth 
to  the  local  bottom,  is  specified.  Platform  location  is  thus 
completely  specified  by  (x,  y ,  z\fi  since  (x,  y)  relative  to  the 
reference  map’s  origin  can  always  be  translated  to  absolute 
positional  coordinates  on  the  earth’s  surface. 


2.1.  Converting  INS  measurements  to  UTM  based  mea¬ 
surements 


The  body,  or  vehicle  reference,  frame  is  defined  as  follows. 
The  positive  x  axis  points  forward  along  the  body  axis  of  the 
vehicle.  The  positive  y  axis  is  toward  the  right  side  and  the 
positive  z  axis  is  down.  The  origin  is  assumed  to  coincide 
with  the  phase  center  of  the  transmit  and  receive  array.  Con¬ 
sider  the  unit  length  vector  u  =  [10  0]'.  This  vector  is  normal 
to  the  face  of  a  forward  looking  sonar  array,  as  is  considered 
here.  A  beam  pointing  in  a  specific  vertical  and  azimuthal  di¬ 
rection  can  be  specified  by  a  pair  of  rotations  of  u.  We  assume 
that  the  receive  beam  is  first  vertically  steered.  The  vertical 
depression  angle  a  is  defined  in  radians  and  is  negative  for 
downward  steer  (i.e.  clockwise  rotations  from  the  positive  x 
axis)  in  the  xz  plane.  This  amounts  to  rotation  of  the  vector 
about  the  y  axis1.  The  rotation  matrix  Ra  is  given  by: 


cosa 

0 

sina 

0 

1 

0 

—sina 

0 

cosa 

(1) 


and  ua  =  Rau.  Next  ua  is  rotated  [3  radians  about  the  2  axis 
to  the  desired  azimuthal  direction.  Clockwise  rotations  from 
the  positive  x  axis  in  the  xy  plane  are  considered  positive. 
The  rotation  matrix  Rp  is  given  by: 


Rf. 3  = 


cos/3  —sin/3  0 
sinf3  cosf3  0 

0  0  1 


(2) 


and  up  =  Rpua. 

The  INS  provides  attitude  measurements  -  heading  (7), 
pitch  (5)  and  roll  (e)  -  describing  the  orientation  of  the  body 
with  respect  to  an  assumed  ‘local’  North-East-Down  (NED) 
frame  [9].  The  positive  x  axis  of  NED  frame  lies  along  North 
in  the  tangent  plane,  positive  y  along  the  East  and  positive 
2  geocentrically  downward.  Thus  the  coordinate  axes  of  the 
NED  frame  implicitly  depend  on  an  assumed  absolute  posi¬ 
tion  (x,  y ,  z).  Specifically,  the  INU  self-estimate  of  its  center 
of  mass  is  used  to  select  the  local  NED  frame  with  respect  to 
which  the  attitude  measurements  are  provided. 

As  the  NED  coordinate  axes  are  virtually  unperturbed  for 
locations  near  the  INU  self-estimate  it  is  reasonable  to  assume 
that  the  specific  NED  frame  selected  by  the  INU  (and  thus  the 
beam  pointing  direction  vector)  is  fixed  for  a  set  of  points  near 
the  INU  self-estimate.  Points  of  interest  include  the  phase 
center,  which  may  be  translated  from  the  INU  center  of  mass, 
as  well  as  all  candidate  positions  considered  in  the  maximum 
likelihood  estimation  process. 

The  beam  pointing  direction  is  completely  specified  with 
respect  to  the  body  frame  through  up.  We  now  wish  to  spec¬ 
ify  this  direction  with  respect  to  the  ‘local’  North-East-Down 

^ome  inertial  navigation  systems  (INS)  alternatively  assume  the  positive 
y  on  the  left  side  and  positive  z  up.  The  Kearfott  INS  which  we  consider 
later  is  such  an  example. 
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(NED)  frame.  By  Euler’s  theorem  [9]  a  sequence  of  three 
rotations  about  coordinate  axes  are  sufficient  to  rotate  one  co¬ 
ordinate  frame  into  the  other  provided  they  share  the  same 
origin.  From  the  previous  discussion  we  can  safely  translate 
the  origin  of  the  NED  frame  from  the  INU  center  of  mass  to 
the  phase  center  such  that  both  the  body  and  NED  frames  use 
the  phase  center  as  the  origin. 

The  ‘local’  NED  frame  can  be  rotated  to  match  the  body 
frame  through  first  rotating  the  NED  frame  in  the  NE  plane 
clockwise  by  7  radians.  This  angle  is  the  vehicle  heading. 
The  new  frame  is  then  rotated  in  the  new  xz  plane  upward 
by  S  radians.  This  angle  is  the  pitch.  Finally  the  new  frame 
is  rotated  in  the  new  yz  plane  clockwise  (as  viewed  from  the 
tail  end  of  the  vehicle)  by  e  radians.  This  angle  is  the  roll. 

Thus  any  vector  u  defined  in  the  NED  frame  can  be  rede¬ 
fined  in  the  body  frame  by  Re  R$  R1  u  where 


Rsy 


R$ 


Re 


cos'y  sin'y  0 
—sinj  cos^f  0 

0  0  1 

cosS  0  sinS 
0  1  0 
sinS  0  cos5 

0  0  1 

1  0  0 

0  cose  sine 
0  —sine  cose 


(3) 

(4) 

(5) 


Selecting  (#1,2/1)  at  the  INU  estimate  of  the  phase  center 
should  lead  to  an  accurate  estimate  of  7. 

The  vector  uned ,  redefined  in  the  rotated  UTM  coordi¬ 
nate  system,  is  denoted  uutm  and  is  given  by 


uutm  =  Rrj  uned 


(8) 


where 


Rv  = 


cosy  —siny  0 
sinr]  cosy  0 

0  0  1 


(9) 


Although  uutm  is  defined  with  respect  to  the  UTM  sys¬ 
tem  we  note  that  the  first  element  of  uutm  corresponds  to 
the  component  of  the  beam  pointing  ray  along  grid  north,  the 
second  element  corresponds  to  the  component  along  grid  east 
and  the  third  element  corresponds  to  the  component  along  the 
downward  z  axis.  A  final  rotation  of  the  coordinate  system  is 
required  to  specify  that  the  easting  component  is  the  x  compo¬ 
nent,  the  northing  component  is  the  y  and  the  upward  normal 
is  the  2  component.  The  vector  v  =  [vx  vyvz\'  reflecting  this 
rotation  is  given  by 


where 


v  =  R  uutm 


R  = 


0  1  0 

1  0  0 

0  0-1 


(10) 


(11) 


Conversely  any  vector  u  defined  in  the  body  frame  can  be  re 

r  1 -1 

Re  R§  R1 


defined  in  the  NED  frame  by 
Let  us  define  uned  as 


u  =  RTRj  Rj  u. 


uned  =  RZ  Rj  Rj  up 


(6) 


Thus  the  beam  direction  is  now  completely  described  by 
the  vector  v. 

3.  ESTIMATING  THE  DISTANCE  TO  THE  BOTTOM 
ALONG  A  BEAM 


The  process  of  determining  vehicle  location  will  utilize 
reference  bathymetric  measurements  registered  to  a  Universal 
Transverse  Mercator  (UTM)  grid.  The  UTM  grid  is  a  projec¬ 
tion  of  the  spherical  earth  onto  a  cylinder.  Each  UTM  zone 
is  centered  on  a  meridian  and  defines  grid  north  along  it  [8]. 
The  origin  of  each  zone  is  at  the  intersection  of  the  equator 
and  the  center  meridian.  Note  that  for  the  UTM  coordinate 
system  the  northing  axis  is  the  y  axis  and  the  easting  the  x 
axis. 

Thus  points  along  the  central  meridian  will  be  represented 
as  points  along  the  y  axis  in  the  grid.  Note  that  for  points 
in  the  zone  not  along  the  central  meridian  the  north  and  east 
directions  corresponding  to  the  local  NED  coordinate  frame 
are  rotated  with  respect  to  grid  north  and  east.  For  points  west 
of  the  central  meridian  true  north  is  east  of  grid  north  and  vice 
versa.  The  rotation  angle  77  can  be  determined  by  determining 
the  UTM  zone  locations  (#1,2/1),  (#2, 2/2)  f°r  two  points  with 
the  same  longitude  but  slightly  different  latitudes  and  defining 
r]  as 


The  intersection  of  the  ray  along  v  with  the  bottom  must  occur 
at  (#0  +  tvx,yo  +  tvy,  zq  +  tvz)  for  some  t.  An  estimate  of 
the  path  length  t,  denoted  t,  can  be  obtained  by  finding  the 
smallest  positive  root  of  the  function 

f(t)  =  Z(t)  -  (z0  +  tvz)  (12) 

where  Z(t)  =  Z(x,y)  is  the  depth  Z  of  the  ocean  floor  at 
(#, 2/)  =  (#0  +  tvx,yo  +  tVy).  The  function  Z(x,y)  is  as¬ 
sumed  to  be  a  bivariate  quadratic  spline  interpolation2  of  the 
N  database  measurements  (#*,  2/i,  z*),  i  =  1 ,  ...,7V.  Tensor 
product  splines  are  well- suited  for  interpolation  of  gridded 
data  as  is  available  in  most  bathymetric  databases.  The  bi¬ 
variate  quadratic  spline  interpolation  is  an  instance  of  a  tensor 
product  spline. 

A  rapid  root  finding  routine  with  guaranteed  convergence 
such  as  Brent’s  method  [7]  can  be  applied  to  obtain  t.  This 
routine  is  known  as  f zero  in  Matlab  [10].  This  is  performed 
for  each  of  the  beams. 

2The  MATLAB  Spline  toolbox  routine  spapi.m  is  used 
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3.1.  Refining  the  estimate:  Solving  for  the  exact  roots  of 
the  quartic 

The  sea  floor  depth  in  each  sub-rectangle  of  the  grid  is  ap¬ 
proximated  by  a  bivariate  polynomial  in  x  and  y  of  degree  n 
in  x  and  m  in  y  [6].  In  our  case  the  degree  in  both  dimensions 
is  equal  to  2,  or  we  have  a  quadratic  in  x  for  y  fixed  and  vice 
versa. 

It  is  clear  that  (x(t) ,  y(t))  falls  within  a  specific  sub-rectangle. 
Denote  the  bivariate  quadratic  polynomial  describing  the  depth 
for  points  in  this  sub-rectangle  as 

2  2 

p(x,y)  =  (x  -  xa)2~l{y  -  Va)2-3-  (13) 

i= 0  j= 0 

where  xa  and  ya  specify  the  location  of  the  lower  left  comer 
of  the  sub-rectangle. 

If  we  consider  only  points  (x,y)  =  (x(t),y(t))  =  (xo  + 
tvx,yo  +  tvy )  the  corresponding  depth  p(t)  is  a  quartic  in  t. 
Thus  the  exact  root  of  interest  must  be  one  of  the  four  roots 
of  the  related  quartic  equation 


the  calculation  of  the  root,  intermediate  quantities  are  some¬ 
times  complex  valued.  We  choose  to  view  all  functions  in¬ 
volved  in  determining  the  root  as  extended  in  a  complex  sense. 
As  algebraic  operations  as  well  as  the  multi-valued  square  and 
cube  rooting  functions  are  analytic  [12]  general  complex  dif¬ 
ferentiation  is  possible.  A  function  which  is  a  composition 
of  other  analytic  functions  is  itself  analytic.  This  allows  ap¬ 
plication  of  the  chain  rule  as  well  as  evaluation  of  specific 
partials  at  points  along  the  real  line  when  required.  However 
imaginary  components  must  ultimately  vanish  as  the  partial 
derivatives  must  be  real  valued. 

The  expressions  for  the  roots,  associated  partial  deriva¬ 
tives  and  details  for  their  evaluation  are  lengthy.  They  shall  be 
included  in  a  more  detailed  correspondence.  The  next  section 
will  reveal  how  these  partial  derivatives  are  used  to  realize  the 
MLE  and  the  CRLB. 


4.  MAXIMUM  LIKELIHOOD  ESTIMATOR  OF 

C Xo,Y0,Z0 ) 


q(t)  =  p(t)  —  ( Z()  +  tvz )  =  at4+bt3  +ct2  +dt+e  =  0.  (14) 
After  some  algebra  the  coefficients  can  be  obtained  as 

a  cioo  vx  Vy 

b  =  2a0o  vxvy2(x o  -  xa)  +  2a0o  vx2vy(y0  -  ya)  + 

«01  VX  Vy  “j“  (ZlO  VX  Vy 

c  =  a0o  vy2(xo  —  xa)2  +  a00  vx2(yo  —  ya)2  + 

4a00  vxvv(x o  -  xa)(y0  -  ya)  +  2a0i  vxvy(x0  -  xa)  + 
dot  vx2(y0  -  ya )  +  2di0  vxvv(y0  -  ya)  + 

dio  Vy2(x0  -  xa)  +  dll  VxVy  +  a02  V2  +  d20  Vy 2 

d  =  2a00  vy(xo  —  xa)2(yo  —  ya)  + 

2a00  vx(x0  -  xa)(y0  -  ya)2  +  dot  vy(x0  -  xa )2  + 

2doi  vx(x0  -  xa)(y0  -  ya)  +  aw  vx(y0  -  ya )2  + 

2di0  vy(x o  -  xa)(y0  -  ya)  +  «u  vx(y0  -  ya)  + 
aiivy(x0  -  xa)  +  2a02  vx(x0  -  xa)  + 

2d2o  vy(y0  ~  ya)  +  ai2  vx  +  a2i  vy  -  vz 
e  =  d00  (x0  —  xa)2(yo  —  ya)2  +  a0i  (x0  —  xa)2(yo  —  ya)  + 
dio  (x0  -  xa)(y0  -  ya)2  +  dn  (x0  -  xa)(y0  -  ya)  + 
d02  (x0  -  xa  f  +  d20  ( yo  -  ya)2  +  dl2  (x0  -  Xa)  + 
d2i  ( Vo  -  Va)  +  d2 2  -  Z0  (15) 

The  quartic  is  the  highest  degree  general  polynomial  whose 
roots  can  be  obtained  via  analytical  expressions.  The  choice 
of  a  quadratic  spline  fit  directly  led  to  this  result.  In  addition, 
we  observe  that  these  expressions  are  explicit  algebraic  func¬ 
tions  [11],  i.e.  functions  of  a  finite  number  of  algebraic  and 
root  extraction  operations. 

It  can  be  seen  that  at  least  two  of  the  four  roots  are  real  val¬ 
ued.  Our  estimate  i  will  correspond  to  one  of  these  roots.  In 


The  time-delay  measurements  from  each  of  the  beams  of  our 
forward-looking  (or  bottom- looking  or  side-scan)  sonar  as  well 
as  the  Doppler  Velocity  Logger  altitude  estimate  form  the  raw 
data  for  the  MLE.  Time  delays  are  converted  to  one-way  dis¬ 
tances  to  the  bottom  through  the  local  sound  speed  -  assumed 
fixed  and  known.  This  is  certainly  a  reasonable  assumption 
as  most  map-matching  applications  operate  over  short  ranges. 
This  is  since  signal  to  noise  ratios  generally  degrade  rapidly 
as  the  distance  to  the  bottom  increases.  The  total  number  of 
measurements  is  denoted  B.  Note  that  given  bathymetric  in¬ 
formation,  the  distance  along  a  beam  is  completely  defined  by 
the  position  (x,y,z)  and  the  beam  pointing  direction  v.  We 
assume  that  v  is  known  for  all  B  measurements  and  that  the 
distance  measurements  are  unbiased. 

Denote  the  B  distance  measurements  as  di, ...,  ds-  As¬ 
suming  all  measurements  are  statistically  independent  and  Gaus¬ 
sian  but  not  necessarily  identically  distributed  leads  to  the  log- 
likelihood  function  L 


l  =  E 


(di  ~  Uf 

2<Ti2 


(16) 


in  which  terms  independent  of  (x,  y ,  z)  have  been  discarded. 

Note  that  the  variance  on  each  measurement  is  dependent 
on  the  sonar  orientation.  Specifically  for  a  bottom  looking 
configuration  the  variance  is  expected  to  be  rather  small  while 
for  a  forward  looking  configuration  the  variance  is  expected  to 
be  rather  large.  The  actual  distance  along  the  beam  to  the  bot¬ 
tom  also  affects  the  variance.  Longer  distances  are  expected 
to  lead  to  larger  variances.  Representative  numbers  for  these 
measurements  must  be  provided  in  order  for  the  MLE  and  the 
CRLB  to  be  useful. 


0-933957-35-1  ©2007  MTS 


The  partial  derivatives  of  L  w.r.t.  x,  y  and  z  are: 


5.  PERFORMANCE 


dL 

dx 

II 

*  1 

c+. 

)  dti 
dx 

(17) 

dL 

dy 

(di  —  ti 

~  2^  rr-2 

i= 1  ^ 

)  dti 
dy 

(18) 

dL 

dz 

(di  —  ti 

^  <Ji2 

i= 1 

)  dti 
dz 

(19) 

The  gradient  is  then  gradL  =  k.  The 

gradient  is  used  to  implement  a  conjugate  gradient  procedure 
to  find  the  maximum  of  L.  The  Polak-Ribiere  update  is  used 
[13].  Iterations  cease  when  L  changes  by  less  than  0.01%. 

Second  order  partials  are  not  required  to  calculated  the 
CRLB  but  are  useful  to  check  whether  a  solution  ( grad  L  = 
0)  is  at  least  a  local  maximum.  Specifically,  if  the  3  x  3  Hes¬ 
sian  matrix  of  L  is  negative  definite  (i.e.  all  eigenvalues  are 
negative)  we  are  at  a  local  maximum. 


4.1.  Fisher  Information  Matrix  and  CRLB  at  (xo,  yo,  zo) 

Every  unbiased  estimator  of  the  true  position  (x,  y ,  z)  that  op¬ 
erates  on  these  B  measurements  must  possess  an  error  co- 
variance  matrix  C  such  that  C  —  J~l  is  non-negative  definite 
provided  J-1  exists.  Here  J  is  the  3x3  Fisher  Information 
Matrix  with  elements: 


Ju 

J22 

J33 

J\2 

J 13 

J23 


—  J2I 

=  J31 

=  J32 


(20) 

(21) 

(22) 

(23) 

(24) 

(25) 

(26) 


The  CRLB  can  reveal  how  well  we  can  estimate  position 
in  a  hypothesized  location,  eg.  at  the  current  best  estimate.  We 
will  see  that  the  implementation  of  the  MLE  via  the  conjugate 
gradient  appears  to  meet  the  CRLB.  In  this  case  J-1  also 
serves  as  the  measurement  noise  covariance  for  incorporation 
into  a  Kalman  Filter. 


Data  were  collected  in  the  Narragansett  Bay,  RI  using  a  48 
beam  Reson  7012  forward-looking  sonar  mounted  on  a  AUV 
equipped  with  the  Kearfott  INS  and  a  Doppler  Velocity  Log¬ 
ger  (DVL).  The  total  number  of  measurements,  denoted  B , 
includes  the  48  sonar  beams  as  well  as  the  DVL  altitude  mea¬ 
surement.  A  rough  estimate  of  the  sonar  depth  is  also  avail¬ 
able.  A  bathymetric  database  with  5  meter  resolution  in  both 
x  and  y  was  used  to  form  the  reference  map.  Sonar  beams 
were  steered  15°  downward  and  spanned  an  azimuthal  sec¬ 
tor  of  120°  with  2.5°  spacing.  The  DVL  altitude  error  has 
a  standard  deviation  of  0.1  m  [14].  The  coefficient  of  vari¬ 
ation  (ratio  of  standard  deviation  to  mean)  of  the  estimated 
time-delays  is  0.05.  Thus  the  Reson  path-length  error  has  a 
standard  deviation  of  0.05£.  Assuming  a  flat-bottom  allows 
us  to  approximate  the  standard  deviation  of  the  error  via  the 
altitude  estimate. 

The  AUV  travels  in  a  north-south  manner  over  a  region 
with  a  relatively  deep  bottom.  This  region,  known  as  “the 
hole”,  is  shown  in  Figure  2.  Gould  Island  is  on  the  left  side 
of  the  figure.  The  point  notes  the  location  at  which  map¬ 
matching  was  performed.  Figure  3  plots  the  48  beam  time 
series  along  with  the  locations  of  the  time-delay  estimates 
(white  *)  in  terms  of  sample  number.  These  estimates  are 
chosen  as  the  peak  magnitude  of  each  time  series.  The  white 
dashed  curve  corresponds  to  the  expected  sample  locations 
based  on  the  INS  estimate.  The  white  solid  curve  corresponds 
to  the  expected  sample  locations  based  on  the  MLE  map¬ 
matching  estimate.  Note  that  the  time-delay  estimates  be¬ 
come  poorer  for  beams  numbered  greater  than  35.  This  is 
due  to  a  lower  signal-to-noise  ratio  in  these  beams.  The  log- 
likelihood  function  L  uses  an  Li  norm  (| di  —  U\)  instead  of 
the  L2  norm  ((di  —  U )2).  The  L\  norm  is  preferred  as  it  is 
less  sensitive  to  outliers  and  sacrifices  little  performance  in 
outlier- free  situations.  It  is  closely  linked  with  the  double¬ 
exponential  distribution  -  a  reasonable  model  for  heavy-tailed 
error. 

The  assumed  location  of  the  AUV  based  on  the  integrated 
INS/D VL  estimate  is  denoted  (xo,  yo,zo).  Here  (xo,  yo)  cor¬ 
respond  to  the  output  of  the  integrated  kalman  filter  and  zo 
is  the  altitude  estimate  corrected  for  tidal  variations  to  match 
the  database  datum.  The  location  (xo,  yo)  is  shown  in  Figure 
4  along  with  the  Circular  Error  Probability  ellipse.  It  is  rea¬ 
sonable  to  assume  that  the  true  location  lies  somewhere  in  the 
CEPqo  ellipse. 

Conjugate  Gradient  methods  require  an  initial  guess.  In 
an  operational  setting  a  reasonable  choice  would  be  (xo,  yo,%o) 
It  is  clear  from  the  CRLB  that  the  resolution  in  the  z  dimen¬ 
sion  exceeds  that  in  x  and  y.  This  is  as  expected  as  the  DVL 
estimate  provides  an  accurate  estimate  of  altitude.  When  the 
maximization  routine  does  not  search  over  z  the  MLE  is  less 
than  5  meters  from  (xo,yo,zo)-  Based  on  J-1  the  2 a  el¬ 
lipse  (coverage  of  87%)  contains  the  MLE.  When  instead  a 
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set  of  possible  altitudes  within  the  range  of  possible  error  are 
pre-selected  and  the  conjugate  gradient  routine  finds  the  best 
(x,  y )  for  a  specific  2  and  the  (x,y,z)  solution  corresponding 
to  the  maximum  likelihood  is  selected,  the  estimate  (denoted 
as  *)  is  less  than  3  meters  from  (#0,  yo,  20  )•  In  this  case  the 
la  ellipse  (coverage  of  39%)  contains  the  MLE.  Results  from 
other  scans,  including  those  taken  in  low-relief  areas,  again 
suggested  that  the  estimator  is  efficient  -  i.e.  meets  the  CRLB 

[15]. 

With  an  fully  operational  DVL  it  is  reasonable  to  assume 
that  an  accurate  estimate  of  2  is  available.  However,  for  long 
straight  runs  the  x  and  y  INS  estimates  will  steadily  degrade. 
As  the  conjugate  gradient  methods  are  not  guaranteed  to  find 
the  global  maximum  it  is  possible  that  an  initial  guess  far  from 
the  true  location  can  result  in  a  poor  estimate.  In  order  to  de¬ 
termine  how  the  estimator  performs  when  the  initial  guess  for 
x  and  y  is  much  poorer  than  (xo,  yo)  the  experiment  was  re¬ 
peated  with  an  intial  choice  of  (xq  +  100,  yo  —  100,  z$).  This 
choice  is  approximately  141  meters  away  from  the  true  loca¬ 
tion  of  the  AUV.  The  result  from  the  ML  map-matching  rou¬ 
tine  is  shown  in  Figure  4  (black  square).  Note  that  the  result 
still  lies  within  the  2 a  ellipse  based  on  the  CRLB  suggesting 
that  the  quality  of  the  initial  guess  is  not  overly  critical  to  the 
successful  operation  of  the  estimator. 

6.  CONCLUSIONS 

Due  to  the  strong  connection  with  the  fundamental  error  in 
the  map-matching  process  and  the  relation  to  the  CRLB  the 
estimator  should  be  practically  equivalent  to  the  minimum 
variance  unbiased  estimator.  A  cursory  examination  of  the 
performance  of  image-based  map-matching  methods  corrob¬ 
orates  this.  An  important  benefit  of  the  explicit  CRLB  is  the 
ability  to  evaluate  the  bound  at  the  INS  estimate.  If  the  maxi¬ 
mum  performance  improvement  from  map-matching  is  poor, 
map-matching  need  not  be  performed  to  save  on  processing. 
As  the  trajectory  is  approximately  known  in  advance,  loca¬ 
tions  where  map-matching  will  be  of  benefit  can  be  selected 
apriori.  The  ML  estimator  and  the  CRLB  are  defined  for  both 
the  single- scan  and  multiple- scan  case.  Relative  position  of 
the  AUV  for  each  scan  must  be  known  precisely  for  multiple 
scan  methods  to  be  accurate.  Thus  the  single  scan  case  ap¬ 
pears  better  suited  for  Kalman  Filter  integration.  These  issues 
will  be  discussed  in  a  detailed  correspondence. 
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Fig.  1.  Geometry 
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Fig.  2.  Narragansett  Bay,  RI  -  test  site 
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Fig.  3.  time  series  for  48  beams  and  time-delay  estimates 
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Fig.  4.  map-matching  results  and  comparison  to  CRLB 
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